//!!!!! PITCHTILT parameter included now
//!!!!! fast return to manual only implimented for Auto => Manual transition
//!!!!! at some point need to do it for the 2 navigation modes (Aile/Yaw) => Manual transition
//!!!!! Boost terms still need to be zeroed in servoMix.c

#include "p30f4011.h"
#include "definesRmat.h"
#include "defines.h"

//	If the state machine selects pitch feedback, compute it from the pitch gyro and accelerometer.

int rtlkick = 0 ;

#define RTLKICK RMAX*(RTL_PITCH_DOWN/57.3)

//!!!!! PITCHTILT parameter to compensate for tail rotor torque
#define PITCHTILTADJUST ((int)((2.0*RMAX*PITCHTILT)/57.3))

int pitchgain = (int)(PITCHGAIN*RMAX) ;
int pitchrate = 0 ;
int pitchkd = (int) (PITCHKD*SCALEGYRO*RMAX) ;

int rudderElevMixGain = (int)(RMAX*RUDDER_ELEV_MIX) ;
int rollElevMixGain = (int)(RMAX*ROLL_ELEV_MIX) ;
int navElevMix ;

int elevInput ;

void pitchCntrl(void)
{
	union longww pitchAccum ;
	
#ifdef TestGains
	flags._.GPS_steering = 1 ;
	flags._.pitch_feedback = 1 ;
#endif
	
	navElevMix = 0;
//!!!!! Follow two calcs are used to mix rudder or aileron into elevator term
//!!!!! Only done in Auto mode, no elevator term in Navigation mode??
	if ( RUDDER_NAVIGATION && flags._.pitch_feedback )
	{
		pitchAccum.WW = __builtin_mulss( rmat[6] , rudderElevMixGain ) ;
		pitchAccum.WW = __builtin_mulss( pitchAccum._.W1 , yaw_control ) << 4 ;
		navElevMix += pitchAccum._.W1 ;
	}

	if ( AILERON_NAVIGATION && flags._.pitch_feedback )
	{
		pitchAccum.WW = __builtin_mulss( rmat[6] , rollElevMixGain ) << 1 ;
		pitchAccum.WW = __builtin_mulss( pitchAccum._.W1 , rmat[6] )  >> 3 ;
		navElevMix += pitchAccum._.W1 ;
	}

	pitchAccum.WW = (	__builtin_mulss( rmat[8] , omegagyro[0] )
					-	__builtin_mulss( rmat[6] , omegagyro[2] ))<<1 ;
	pitchrate = pitchAccum._.W1 ;

//!!!!! Navigation mode calc, but is it doing anything???	
//!!!!! did we kill RTLKICK?  Do not see it used in standard MP code
	if ( (RUDDER_NAVIGATION || AILERON_NAVIGATION) && flags._.GPS_steering )
	{
		rtlkick = RTLKICK ;
	}
	else
	{
		rtlkick = 0 ;
	}
//!!!!! This is where we check if we were in Auto but just switched to Manual	
	if ( PITCH_STABILIZATION && flags._.pitch_feedback && (pwIn[MODE_SWITCH_INPUT_CHANNEL] > MODE_SWITCH_THRESHOLD_LOW))
	{
	//!!!!! include PITCHTILT offset, pitchAltitudeAdjust only includes altitude terms
		pitchAccum.WW = 	__builtin_mulss( rmat[7] - PITCHTILTADJUST + pitchAltitudeAdjust, pitchgain ) 
						+	__builtin_mulss( pitchkd , pitchrate ) ;
	}
	else
	{
	//!!!!! no additional code needed for fast return to manual for Auto mode but
	//!!!!! need to zero boost terms in servoMix.c
		pitchAccum.WW = 0 ;
	}
	
	pitch_control = (long)pitchAccum._.W1 + navElevMix ;
	// Servo reversing is handled in servoMix.c
	
	return ;
}
